-- 描述: 将点位信息字符串解析为点位信息
-- 输入: 需要解析点位字符串 LocStr
-- 输出: 点位信息			mLoc
function AnalysisLoc(LocStr)
	
	local mLocArr = Split(LocStr,",")

	local mLoc = Location.New(tonumber(mLocArr[1]),tonumber(mLocArr[2]),tonumber(mLocArr[3]),0,180,tonumber(mLocArr[4]))

	return mLoc
end

-- 描述: 将点位信息字符串解析为点位信息(根据实际情况添加点位)
-- 输入: 无
-- 输出: 无
function SetLoc()
	
	SafeLoc = AnalysisLoc(GString[1])
	SafeLoc.Config = 2
	
	print("SafeLoc:")
	print(SafeLoc)
	
	PickLoc1 = AnalysisLoc(GString[2])
	PickLoc1.Config = 2
	
	print("PickLoc1:")
	print(PickLoc1)
	
	PickWDownLoc1 = AnalysisLoc(GString[3])
	PickWDownLoc1.Config = 2
	
	print("PickWDownLoc1:")
	print(PickWDownLoc1)
	
	PickWUpLoc1 = AnalysisLoc(GString[4])
	PickWUpLoc1.Config = 2
	
	print("PickWUpLoc1:")
	print(PickWUpLoc1)
	
	PickLoc2 = AnalysisLoc(GString[5])
	PickLoc2.Config = 2
	
	print("PickLoc2:")
	print(PickLoc2)
	
	PickWDownLoc2 = AnalysisLoc(GString[6])
	PickWDownLoc2.Config = 2
	
	print("PickWDownLoc2:")
	print(PickWDownLoc2)
	
	PickWUpLoc2 = AnalysisLoc(GString[7])
	PickWUpLoc2.Config = 2
	
	print("PickWUpLoc2:")
	print(PickWUpLoc2)
	
	PlaceWait = AnalysisLoc(GString[8])
	PlaceWait.Config = 2
	
	print("PlaceWait:")
	print(PlaceWait)
	
	PlaceMiddle = AnalysisLoc(GString[9])
	PlaceMiddle.Config = 2
	
	print("PlaceMiddle:")
	print(PlaceMiddle)
	
	PlaceOffect = AnalysisLoc(GString[10])
	PlaceOffect.Config = 2
	
	print("PlaceOffect:")
	print(PlaceOffect)
	
	PlaceUp = AnalysisLoc(GString[11])
	PlaceUp.Config = 2
	
	print("PlaceUp:")
	print(PlaceUp)
	
	PlaceUpOffect = AnalysisLoc(GString[12])
	PlaceUpOffect.Config = 2
	
	print("PlaceUpOffect:")
	print(PlaceUpOffect)
	
	
	PlaceUpLeft = AnalysisLoc(GString[13])
	PlaceUpLeft.Config = 2
	
	print("PlaceUpLeft:")
	print(PlaceUpLeft)
	
	PassLoc1 = AnalysisLoc(GString[14])
	PassLoc1.Config = 2
	
	print("PassLoc1:")
	print(PassLoc1)
	
	PassLoc2 = AnalysisLoc(GString[15])
	PassLoc2.Config = 2
	
	print("PassLoc2:")
	print(PassLoc2)
	
	PassLoc3 = AnalysisLoc(GString[16])
	PassLoc3.Config = 2
	
	print("PassLoc3:")
	print(PassLoc3)
	
	PassLoc4 = AnalysisLoc(GString[17])
	PassLoc4.Config = 2
	
	print("PassLoc4:")
	print(PassLoc4)
	
	PassLoc5 = AnalysisLoc(GString[18])
	PassLoc5.Config = 2
	
	print("PassLoc5:")
	print(PassLoc5)

end

-- 描述: 移动到安全高度,并检测机器人的位置
-- 输入: 无
-- 输出: 无
function MoveToSafeHeight()
	Stt = 0
	local mHereLoc = Location.New()
	local mHereLocRoll = Location.New()
	mHereLoc:Here(1)
	mHereLocRoll:Here(1)
	
	if(mHereLoc.Y>300)then
		print("机器人不在安全区域，请将机器人移动到安全区域内")
		GDouble[3]=-20009
		IO.Set(DO_ALARM,1)
		Stt = -1
		
	elseif(mHereLoc.Y >=31) and(mHereLoc.Y<=300) then
		print("机器人在卡簧区域内")
		mHereLoc.Z = -58
		mHereLocRoll.Roll = 47
		mHereLocRoll.Z = -58
		-- 先提升高度
		Move.Line(mHereLoc)
		Move.WaitForEOM()
		print("已经到安全高度")
		-- 再校正角度
		Move.Line(mHereLocRoll)
		Move.WaitForEOM()
		print("已经校正点位")

	elseif (mHereLoc.Y<31) then 
		mHereLoc.Z = -10
		-- 提升高度
		Move.Line(mHereLoc)
		Move.WaitForEOM()
	end
	
	return Stt
	
end

-- 描述: 从当前点位抬升至安全高度,然后移动到目标点上方,最后到达目标点
-- 输入: 无
-- 输出: 无
function UpToLoc(mLoc)
	
	local mAppLoc = Location.New()
	mAppLoc = mLoc:Clone()
	mAppLoc.Z = -10
	
	MoveToSafeHeight()
	
	Move.Line(mAppLoc)
	Move.WaitForEOM()
	
	Move.Line(mLoc)
	Move.WaitForEOM()

end